%> @file study_mpc_constr_DeDonaoutdisturbreject_ccsparse_optimized.m
%> @brief Constrained MPC MIMO controller with the Hessian calculations based on the described in De Dona book.
%>
%> @author Mikhail Konnik
%> @date   10 January 2012
%>
%======================================================================
%> @param y		= output from the plant, a vector of [m x 1] size.
%>
%> @retval u_new	=  output from the control (control signal) is a vector of [n x 1] outputs.
% ======================================================================
function u_new = study_mpc_constr_DeDonaoutdisturbreject_ccsparse_optimized(y)

Np=2;	%% state prediction horizon
Nc=1;	%% control signals prediction horizon


global A_e B_e C_e G row_B_e col_B_e cc iter;

% X_hat is stored locally in this embedded function,
% we need to define its initial condition.

persistent X_hat F_times_X_hat;
if (exist('X_hat','var')&&(isempty(X_hat)))
    X_hat = sparse(size(B_e,1),1);

    [row_B_e,col_B_e] = size(B_e);
    F_times_X_hat = sparse(size(B_e,1),1);
end


persistent u_old;
if (exist('u_old','var')&&(isempty(u_old)))
    u_old =  sparse(size(B_e,1),1);
end



%%%%%%%%%%%%%%%  START: This is the Cold Start for the MPC - run once to calculate the H and F %%%%
persistent H F inv_H;
if (exist('H','var')&&(isempty(H)))
nameformat = cc.plant.size;

[H, F] = cc_mpc_coldstart_matrixevaluation_sparse(A_e,B_e,C_e,Nc,Np);


H = tool_matrix_thresholder(H, max(H(:))*cc.matrix.Hessian.thresh); nameformat = strcat(nameformat,'Hthresh');
H = full(H);
 
F = tool_matrix_thresholder(F, max(F(:))*cc.matrix.F.thresh); nameformat = strcat(nameformat,'Fthresh');
F = full(F);


inv_H=H\eye(length(diag(H)));

inv_H = tool_matrix_thresholder(inv_H, ...
    max(inv_H(:))*cc.matrix.inv_Hessian.thresh); nameformat = strcat(nameformat,'inv_Hessian_thresh'); 
% inv_H = sparse(inv_H);   %%% <---- Do not use this with Dantzig-Wolfe! It slowes the algorithm down 
inv_H = full(inv_H); 

% 
% % %%%%%%%% <-------- This is for the research purpose and displaying the structure of Hessian
load('colourmap_for_hessian','mycmap');
% 
figure(122), imagesc(tool_matrix_normaliser(full(F))), colormap(jet), colorbar; 
cc.matrix.F.totalelements = size(F,1)*size(F,2);
cc.matrix.F.nnz = nnz(F(:));
cc.matrix.F.sparsity = round( ((cc.matrix.F.totalelements - cc.matrix.F.nnz)/cc.matrix.F.totalelements )*100) ;
xlabel(strcat('Total elements=',num2str(cc.matrix.F.totalelements),', nonzero=',num2str(cc.matrix.F.nnz),' (',num2str( cc.matrix.F.sparsity ),'% sparse)' ));
set(122,'Colormap',mycmap); caxis([-1 1]);
% viz_print_figure_to_eps(122,strcat('mpc_MIMO_unconstrained_',nameformat,'_Fmatrix_colour'));
save(strcat('data_mpc_MIMO_',nameformat,'_Fmatrix.mat'), 'F');
% 
figure(123), imagesc(tool_matrix_normaliser(full(H))), colormap(jet), colorbar;
cc.matrix.Hessian.totalelements = size(H,1)*size(H,2);
cc.matrix.Hessian.nnz = nnz(H(:));
cc.matrix.Hessian.sparsity = round(( (cc.matrix.Hessian.totalelements - cc.matrix.Hessian.nnz)/cc.matrix.Hessian.totalelements )*100) ;
xlabel(strcat('Total elements=',num2str(cc.matrix.Hessian.totalelements),', nonzero=',num2str(cc.matrix.Hessian.nnz),' (',num2str( cc.matrix.Hessian.sparsity ),'% sparse)' ));
set(123,'Colormap',mycmap); caxis([-1 1]);
% viz_print_figure_to_eps(123,strcat('mpc_MIMO_unconstrained_',nameformat,'_Hessianmatrix_colour'));
save(strcat('data_mpc_MIMO_',nameformat,'_Hessianmatrix.mat'), 'H');
% math_spectral_radius(full(H))
% stat_matrix_condition_number(full(H))
% [U,V,H] = svd(full(H));
% diag(V)
% 
% figure(124), imagesc(tool_matrix_normaliser(full(inv_H))), colormap(jet), colorbar;
cc.matrix.inv_Hessian.totalelements = size(inv_H,1)*size(inv_H,2);
cc.matrix.inv_Hessian.nnz = nnz(inv_H(:));
cc.matrix.inv_Hessian.sparsity = round( ((cc.matrix.inv_Hessian.totalelements - cc.matrix.inv_Hessian.nnz)/cc.matrix.inv_Hessian.totalelements )*100) ;
% xlabel(strcat('Total elements=',num2str(cc.matrix.inv_Hessian.totalelements),', nonzero=',num2str(cc.matrix.inv_Hessian.nnz),' (',num2str( cc.matrix.inv_Hessian.sparsity   ),'% sparse)' ));
% set(124,'Colormap',mycmap); caxis([-1 1]);
% viz_print_figure_to_eps(124,strcat('mpc_MIMO_unconstrained_',nameformat,'_inverseHessianmatrix_colour'));
save(strcat('data_mpc_MIMO_',nameformat,'_inverseHessianmatrix.mat'), 'inv_H');
% 
% % %%%%%%%%% <-------- This is for the research purpose and displaying the structure of Hessian

end
%%%%%%%%%%%%%%%  END: This is the Cold Start for the MPC - run once to calculate the H and F %%%%





%%%%%%%%%%%%%%%  # START: Contraints on Control amplitude u %%%%
persistent gamma M row_M;
if (exist('M','var')&&(isempty(M)))
       [gamma, M] = cc_mpc_coldstart_contraintsevaluation(Nc,cc.constraints.u_max,cc.constraints.u_min,col_B_e);

%     gamma = sparse(gamma);
    M = sparse(M);
	[row_M,col_M]=size(M);
end
%%%%%%%%%%%%%%% #### END: Contraints on Control amplitude u %%%%



%%%%% ### START: FINALLY, calculating the optimal control signal
F_times_X_hat = F*X_hat;
 


    %%%%%%%%%%%%%%%  # START: Accelerating the qpdantz algorithm for qpdantz_kmv_accelerated.m %%%%
    persistent maxiter xmin a TAB rhsc mnu nc ibi ili
    if (exist('xmin','var')&&(isempty(xmin)))

%     maxiter=20;
%     maxiter=35;
    maxiter=50;

    xmin=-1e3*ones(size(F_times_X_hat(:)));
    
    a=-H*xmin(:);    % This is a constant term that adds to the initial basis
                 % in each QP.

    TAB=[-inv_H inv_H*M';M*inv_H -M*inv_H*M'];

    rhsc=gamma(:)-M*xmin(:);
    
    
mnu=length(F_times_X_hat);
nc=length(gamma);
    
ibi=-[1:mnu+nc]';
ili=-ibi;
    end
    %%%%%%%%%%%%%%%  #### END: Accelerating the qpdantz algorithm for qpdantz_kmv_accelerated.m %%%%


     u_new = qpdantz_kmv_accelerated(H,F_times_X_hat,M,gamma,xmin,maxiter,a,inv_H,TAB,rhsc,mnu,nc,ibi,ili);

% u_new = u_new(1:col_B_e,1);
u_old = u_new;
%%%%% ### ## END: FINALLY, calculating the optimal control signal







%%%%% START: Steady state observer
persistent K_ob;

if (exist('K_ob','var')&&(isempty(K_ob)))
    ETA = 10^(-6)*eye(size(B_e,2));  %% <--- Deliberately made full, not to confuse DARE in dlqr
    GG = G*G'+ eps*eye(size(A_e,1)); %% <--- Deliberately made full, not to confuse DARE in dlqr


    [K_ob, S, e] = dlqr(A_e',C_e',GG,ETA); %%% here, the matrices are converted from Sparse to Full.
    %%%%% Thresholding the gains that are too small to impact the states %%%%%%
    K_ob = tool_matrix_thresholder(K_ob, max(K_ob(:))*10^(-3));

    K_ob = sparse(K_ob);
    K_ob = K_ob'; 
end

z1 = A_e*X_hat;
z2 = (y-C_e*X_hat);
z3 = B_e*u_new; %%% in the case of regulator, the U is more stable than Delta U
X_hat=z1+K_ob*z2+z3; %%%% using the observer.
%%%%% ## END: Steady state observer


